#include <string.h>
#include <unistd.h>
#include <netinet/in.h>

#include <glib.h>
#include <gst/gst.h>
#include <gst/rtp/gstrtpbuffer.h>

#define DEST_MAXSIZE 128

struct rtpdemux_s { 
  GstElement *pipeline;
  GstElement *adder;
  GstCaps *audio_cap;
};

static void new_payload_type (GstElement * element, gint pt, GstPad *pad, gpointer data)
{
  struct rtpdemux_s *comp = (struct rtpdemux_s*) data;
  GstElement *decoder, *depayloader, *sink;
  GstElement *converter, *resample;
  GstCaps *caps;
  GstPad *adderPad;
  gchar *padname = gst_pad_get_name (pad);

  g_print("New payload %d\n",pt);
  
  gst_element_set_state (comp->pipeline, GST_STATE_PAUSED);
  //fprintf(stderr, "Caps: %s\n", gst_caps_to_string( gst_pad_get_caps(pad) ) );
  switch (pt) {
  case 11:
  //case 10:
  //case 111:
  case 112:
  //case 113:
  case 114:
  //case 115:
  case 116:
  //case 117:
  case 122:
    // decoder and depayloader
    depayloader = gst_element_factory_make("rtpl16depay", NULL);
    if (!depayloader) {
      printf("unable to create a l16depayload.\n");
      exit(1);
    }  
    
    converter = gst_element_factory_make("audioconvert", "convert_l16_%d");
    resample = gst_element_factory_make("audioresample", "resample_l16_%d");

    adderPad = gst_element_get_request_pad(comp->adder, "sink%d");

    if (!adderPad)
      g_error("Decoder couldn't be loaded\n");
    gst_pad_fixate_caps(gst_element_get_pad(converter, "src"), comp->audio_cap);

    // add receive elements to the pipeline and link them
    gst_bin_add_many(GST_BIN(comp->pipeline), depayloader, converter, resample, NULL);
    if (!gst_element_link_pads(element, padname, depayloader, "sink"))
      g_error("%s: Error linking L16 input->depayloader\n", __func__);
    if (!gst_element_link_pads(depayloader, "src", converter, "sink"))
      g_error("%s: Error linking L16 depayloader->decoder\n", __func__);
    gst_element_link_pads(converter, "src", resample, "sink");
    gst_pad_link( gst_element_get_pad(resample, "src"), adderPad);

  break;

  case 3:
  case 118:
    // decoder and depayloader
    decoder = gst_element_factory_make("gsmdec", NULL);
    depayloader = gst_element_factory_make("rtpgsmdepay", NULL);
    converter = gst_element_factory_make("audioconvert", "convert_gsm_%d");
    resample = gst_element_factory_make("audioresample", "resampler_gsm_%d");

    if (!decoder) {
      printf("unable to create a gsmdec.\n");
      exit(1);
    }  
    if (!depayloader) {
      printf("unable to create a rtpgsmdepay.\n");
      exit(1);
    }  
    g_object_set(G_OBJECT(depayloader), "queue-delay", 0, NULL);

    adderPad = gst_element_get_request_pad(comp->adder, "sink%d");
    gst_pad_fixate_caps(gst_element_get_pad(converter, "src"), comp->audio_cap);

    // add receive elements to the pipeline and link them
    gst_bin_add_many(GST_BIN(comp->pipeline), decoder, depayloader, converter,  resample, NULL);
    if (! (
      gst_element_link_pads(element, padname, depayloader, "sink") &&
      gst_element_link_pads(depayloader, "src", decoder, "sink") &&
      gst_element_link_pads(decoder, "src", converter, "sink") ))
      g_error("%s: Error linking GSM decoder pipeline\n", __func__);
    gst_element_link_pads(converter, "src", resample, "sink");
    gst_pad_link( gst_element_get_pad(resample, "src"), adderPad);
  break;

  default:
    g_print("Unknown PT %d\n", pt);
  }

  g_free(padname);
  gst_element_set_state (comp->pipeline, GST_STATE_PLAYING);
}


static void payload_type_change(GstElement * element, gint pt, gpointer data)
{
  g_debug("%s: active PT change to %d\n", __func__, pt);

}

/* Called when a new SSRC arrives */
void new_ssrc(GstElement* element,
                                    gint ssrc,
                                    GstPad* pad,
                                    gpointer data
                                    ){
//  struct rtpdemux_state_s *self = (struct rtpdemux_state_s *)data;
//  GstElement *pipeline = self->pipeline;
  struct rtpdemux_s *comp = (struct rtpdemux_s*) data;
  GstElement *decoder, *depayloader, *sink;

  g_debug("%s: adding depayloader chain.\n", __func__);

  /* pause the pipeline for to the add the pad */
  gst_element_set_state (comp->pipeline, GST_STATE_PAUSED);
  //gst_element_set_state (element, GST_STATE_PAUSED);
  /* rtpdemux */
  /* -------------------- */
  GstElement *rtpdemux = gst_element_factory_make ("rtpdemux", NULL);
  if (!rtpdemux) {
    g_print ("rtpdemux not found, check your install\n");
    return;
  }
  g_signal_connect (G_OBJECT (rtpdemux), "new-payload-type", G_CALLBACK (new_payload_type), comp);
  g_signal_connect (G_OBJECT (rtpdemux), "payload-type-change", G_CALLBACK (payload_type_change), comp);

  gst_bin_add_many( GST_BIN(comp->pipeline), rtpdemux, NULL);
  gst_pad_link(pad, gst_element_get_pad(rtpdemux , "sink"));

  /* restart the pipeline */
  //gst_element_set_state (element, GST_STATE_PLAYING);
  gst_element_set_state (comp->pipeline, GST_STATE_PLAYING);
}

void remove_ssrc(GstElement* object,
                                    gint ssrc,
                                    GstPad* pad,
                                    gpointer data
                                    ){

}

/* 
 * Bus handler
 */
static gboolean bus_call (GstBus     *bus,
			  GstMessage *msg,
			  gpointer    data)
{
  GMainLoop *loop = data;
  gchar *debug;
  GError *err;
  
  switch (GST_MESSAGE_TYPE (msg)) {
    case GST_MESSAGE_EOS:
      g_print ("End-of-stream\n");
      //g_main_loop_quit (loop);
      break;
    case GST_MESSAGE_ERROR: {

      gst_message_parse_error (msg, &err, &debug);
      g_free (debug);

      g_print ("Error: %s\n", err->message);
      g_error_free (err);

      g_main_loop_quit (loop);
      break;
    }
    case GST_MESSAGE_STATE_CHANGED:
      break;

    case GST_MESSAGE_INFO:
      gst_message_parse_info(msg, &err, &debug);
      g_free(debug);
      g_print("Info: %s\n", err->message);
      g_error_free(err);
      break;

    default:
      g_print("Message %d\n", GST_MESSAGE_TYPE(msg));
      break;
  }

  return TRUE;
}

/* 
 * Main application
 */
int 
main (int   argc,
      char *argv[]) 
{
  GstElement *bin, *rtpbin, *encoder, *payloader, *decoder, *depayloader, *demuxer;
  GMainLoop *loop;

  setenv("JRTPLIBC_ACCEPT_OWN", "1", 1);

  gst_init (&argc, &argv);

  struct rtpdemux_s *comp = g_new0(struct rtpdemux_s, 1);

  guint32 pt_gsm = GST_RTP_PAYLOAD_GSM;  /* PT=3 => GSM */
  guint32 pt_gsm_16k = 118;  /* PT=3 => GSM */
  guint32 pt_l16_8k_mono = 122;
  guint32 pt_l16_8k_stereo = 111;
  guint32 pt_l16_16k_mono = 112;
  guint32 pt_l16_16k_stereo = 113;
  guint32 pt_l16_32k_mono = 114;
  guint32 pt_l16_32k_stereo = 115;
  guint32 pt_l16_44k_mono = 11;
  guint32 pt_l16_44k_stereo = 10;
  guint32 pt_l16_48k_mono = 116;
  guint32 pt_l16_48k_stereo = 117;
  char destination[DEST_MAXSIZE];
  int port, rtp_sockfd, rtcp_sockfd = -1;
  struct sockaddr_in rtp_addr;
  GstCaps *caps;
  GHashTable *pt_map = g_hash_table_new_full (g_direct_hash,
					      g_direct_equal,
					      NULL,
					      (GDestroyNotify) gst_caps_unref);

  /* initialize the PT<->caps hash table */
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 8000, NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_gsm), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 16000, NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_gsm_16k), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 8000, 
      "channels", G_TYPE_INT, 1, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_8k_mono), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 8000, 
      "channels", G_TYPE_INT, 2, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_8k_stereo), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 16000, 
      "channels", G_TYPE_INT, 1, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_16k_mono), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 16000, 
      "channels", G_TYPE_INT, 2, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_16k_stereo), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 32000, 
      "channels", G_TYPE_INT, 1, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_32k_mono), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 32000, 
      "channels", G_TYPE_INT, 2, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_32k_stereo), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 44000, 
      "channels", G_TYPE_INT, 1, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_44k_mono), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 44000, 
      "channels", G_TYPE_INT, 2, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_44k_stereo), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 48000, 
      "channels", G_TYPE_INT, 1, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_48k_mono), (gpointer) caps);
  caps = gst_caps_new_simple ("application/x-rtp",
      "clock-rate", G_TYPE_INT, 48000, 
      "channels", G_TYPE_INT, 2, 
      NULL);
  g_hash_table_insert (pt_map, GINT_TO_POINTER(pt_l16_48k_stereo), (gpointer) caps);

  loop = g_main_loop_new(NULL, FALSE);

  if (argc != 3)
  {
      printf("Usage : gsmsendrecv <host> <port>\n");
      return 0;
  }

  /* set the local port */
  port = atoi (argv[2]);

  /* create and bind a socket for RTP and RTCP */
  memset(&rtp_addr, sizeof(struct sockaddr_in), 0);
  rtp_sockfd = socket (AF_INET, SOCK_DGRAM, 0);
  rtcp_sockfd = socket (AF_INET, SOCK_DGRAM, 0);
  if (rtp_sockfd > 0 && rtcp_sockfd > 0) {
    int res;
    rtp_addr.sin_family = AF_INET;
    rtp_addr.sin_port = htons (port);
    rtp_addr.sin_addr.s_addr = INADDR_ANY;

    res = bind(rtp_sockfd, (struct sockaddr*)&rtp_addr, sizeof(rtp_addr));
    if (res == 0) {
      fprintf(stderr, "Succesfully bound to local RTP port %d.\n", port);
    } else {
      fprintf(stderr, "Unable to bind to local RTP port %d.\n", port);
      rtp_sockfd = -1;
    }

    memset(&rtp_addr, sizeof(struct sockaddr_in), 0);
    rtp_addr.sin_family = AF_INET;
    rtp_addr.sin_port = htons (port + 1);
    rtp_addr.sin_addr.s_addr = INADDR_ANY;
    res = bind(rtcp_sockfd, (struct sockaddr*)&rtp_addr, sizeof(rtp_addr));
    if (res == 0) {
      fprintf(stderr, "Succesfully bound to local RTCP port %d.\n", port + 1);
    } else {
      fprintf(stderr, "Unable to bind to local RTCP port %d.\n", port + 1);
      rtcp_sockfd = -1;
    }

    // Join multicast group
    struct ip_mreq mreq;
    mreq.imr_multiaddr.s_addr = inet_addr(argv[1]);
    mreq.imr_interface.s_addr = INADDR_ANY;
    setsockopt(rtp_sockfd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
  }

  bin = gst_pipeline_new("pipe");
  comp->pipeline = bin;
  gst_bus_add_watch (gst_pipeline_get_bus (GST_PIPELINE (bin)),
		     bus_call, loop);
  fprintf(stderr,"bin: %p\n",bin);  
  
  // first we create our rtpbin object
  rtpbin = gst_element_factory_make("rtpbin", "rtpbin");
  fprintf(stderr,"rtpbin: %p\n",rtpbin);  
  snprintf(destination, DEST_MAXSIZE, "%s:%s", argv[1], argv[2]);
  g_object_set(G_OBJECT(rtpbin), "destinations", destination, NULL);
  g_object_set(G_OBJECT(rtpbin), "rtcp-support", TRUE, NULL);
  g_object_set(G_OBJECT(rtpbin), "localport", port, NULL);
  g_object_set(G_OBJECT(rtpbin), "pt-map", pt_map, NULL);
  if (rtp_sockfd != -1)
    g_object_set(G_OBJECT(rtpbin), "rtp_sockfd", rtp_sockfd, NULL);
  if (rtcp_sockfd != -1)
    g_object_set(G_OBJECT(rtpbin), "rtcp_sockfd", rtcp_sockfd, NULL);
  
  gst_bin_add_many(GST_BIN(bin), rtpbin, NULL);
 /* 
  // then our encoder/payloader
  encoder = gst_element_factory_make("gsmenc", "gsmenc");
  payloader = gst_element_factory_make("rtpgsmpay", "rtpgsmpay");

  // add both to the piepline and link them
  gst_bin_add_many(GST_BIN(bin), rtpbin, encoder, payloader, NULL);
  gst_element_link_pads(encoder, "src", payloader, "sink");
  gst_element_link_pads(payloader, "src", rtpbin, "sink%d");

  // our test src and link it to the encoder
  GstElement *testsrc = gst_element_factory_make("alsasrc", "alsasrc");
  g_object_set(G_OBJECT(testsrc), "blocksize", 320, NULL);
  gst_bin_add_many(GST_BIN(bin), testsrc, NULL);
  gst_element_link_pads(testsrc, "src", encoder, "sink");
*/
  // Demuxer
  demuxer = gst_element_factory_make("rtpssrcdemux", "rtpssrcdemux");
  if (! demuxer){
    fprintf(stderr, "demuxer not set\n");
    return -1;
  }

  g_signal_connect (G_OBJECT(demuxer), "new-ssrc", G_CALLBACK(new_ssrc), comp);
  g_signal_connect (G_OBJECT(demuxer), "remove-ssrc", G_CALLBACK(remove_ssrc), comp);

  // decoder and depayloader
/*  decoder = gst_element_factory_make("gsmdec", "gsmdec");
  depayloader = gst_element_factory_make("rtpgsmdepay", "rtpgsmdepay");
  if (!decoder) {
      printf("unable to create a gsmdec.\n");
      exit(1);
  }  
  if (!depayloader) {
      printf("unable to create a rtpgsmdepay.\n");
      exit(1);
  }  
  g_object_set(G_OBJECT(depayloader), "queue-delay", 0, NULL);

  // add receive elements to the pipeline and link them
  gst_bin_add_many(GST_BIN(bin), decoder, depayloader, NULL);
  gst_element_link_pads(rtpbin, "src%d", depayloader, "sink");
  gst_element_link_pads(depayloader, "src", decoder, "sink");

  // our alsasink and link it to the encoder
  GstElement *testsink = gst_element_factory_make("alsasink", "alsasink");
  g_object_set(G_OBJECT(testsink), "sync", TRUE, NULL);
  gst_bin_add_many(GST_BIN(bin), testsink, NULL);
  gst_element_link_pads(decoder, "src", testsink , "sink");
*/
/*
 * Test file sink
  GstElement *testsink = gst_element_factory_make("filesink", "filesink");
  g_object_set(G_OBJECT(testsink), "location", "/tmp/data", NULL);
  gst_bin_add_many(GST_BIN(bin), testsink, NULL);
  gst_element_link_pads(rtpbin, "src%d", testsink , "sink");
*/
  gst_bin_add_many(GST_BIN(bin), demuxer, NULL);
  gst_element_link_pads(rtpbin, "src%d", demuxer, "sink");

  GstElement* adder = gst_element_factory_make("asyncmixer", "asyncmixer");
  if (!(adder && GST_IS_ELEMENT(adder)))
    g_error("Could not create asyncmixer\n");
  comp->adder = adder;

  GstElement* output = gst_element_factory_make("alsasink", "output");
  g_object_set(G_OBJECT(output), "sync", FALSE, NULL);

  comp->audio_cap = gst_caps_new_simple ("audio/x-raw-int",
      "rate", G_TYPE_INT, 48000, 
      "channels", G_TYPE_INT, 1, 
      "width", G_TYPE_INT, 32, 
      "depth", G_TYPE_INT, 32, 
      "endianness", G_TYPE_INT, 1234, 
      "signed", G_TYPE_BOOLEAN, TRUE, 
      NULL);
    
  GstPad * pad = gst_element_get_pad(adder, "src");
  if (!pad){
    g_error("Could not get source pad\n");
  }
  gst_pad_fixate_caps(pad, comp->audio_cap);
  gst_pad_fixate_caps(gst_element_get_pad(output, "sink"), comp->audio_cap);

  gst_bin_add_many(GST_BIN(bin), adder, output, NULL);
  if (!gst_element_link_pads(adder, "src", output, "sink"))
    g_error("Could not link mixer to sink\n");

  // let's start playing
  gst_element_set_state (bin, GST_STATE_PLAYING);

  g_main_loop_run(loop);

  return 0;
}
